/*
 * position.c
 *
 *  Created on: May 22, 2021
 *      Author: kirk_
 */

#include "position.h"

#include "velocity.h"

void PositionInit(void* argument)
{
	uint32_t Period;

	(void)Period;

	/* Get arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Fetch_Period;

	/* Init params */
	KD_FloatPID_Init(&PositionX, PositionDownlink);
	KD_FloatPID_SetIndex(&PositionX, KD_FloatPID_IndexP, 0.00);
	KD_FloatPID_SetIndex(&PositionX, KD_FloatPID_IndexI, 0.00);
	KD_FloatPID_SetIndex(&PositionX, KD_FloatPID_IndexD, 0.00);
	KD_FloatPID_SetIndex(&PositionX, KD_FloatPID_Target, 0.00);
	KD_FloatPID_SetIndex(&PositionX, KD_FloatPID_Tolerance, 0.00);
	/* Init params */
	KD_FloatPID_Init(&PositionY, PositionDownlink);
	KD_FloatPID_SetIndex(&PositionY, KD_FloatPID_IndexP, 0.00);
	KD_FloatPID_SetIndex(&PositionY, KD_FloatPID_IndexI, 0.00);
	KD_FloatPID_SetIndex(&PositionY, KD_FloatPID_IndexD, 0.00);
	KD_FloatPID_SetIndex(&PositionY, KD_FloatPID_Target, 0.00);
	KD_FloatPID_SetIndex(&PositionY, KD_FloatPID_Tolerance, 0.00);

	/* Test param */

	PositionX.Denominator = -1000;
	PositionX.indexP = 2.00;
	PositionX.indexI = 0.00;
	PositionX.indexD = 0.00;
	PositionX.Tolerance = 0.10;
	PositionX.Target = 0.00;

	PositionY.Denominator = -1000;
	PositionY.indexP = 2.00;
	PositionY.indexI = 0.00;
	PositionY.indexD = 0.00;
	PositionY.Tolerance = 0.10;
	PositionY.Target = 0.00;

	return;
}

float PositionConvert(int32_t position)
{
	return ((float)position) / Position_Denominator;
}

void PositionDownlink(KD_FloatPID_t* pid)
{
	if(&PositionX == pid)
	{
		VelocityIncTarget(0, pid->Increment);
		VelocityIncTarget(3, pid->Increment);
	}
	else if(&PositionY == pid)
	{
		VelocityIncTarget(1, pid->Increment);
		VelocityIncTarget(2, pid->Increment);
	}

	return;
}

